#include "laser_obsta_avoid.hpp"

namespace AgvObstaAvoid {

LaserObstaAvoid::LaserObstaAvoid():
  scan_check_size_(true),
  num_frame_(3),
  last_stop_(false),
  vehicle_forward_(false)
{
}

// void LaserObstaAvoid::init()
// {
//     // Initialize subscribers
//     // path_sub_ = this->create_subscription<agv_msgs::msg::AgvAutoStatus>(
//     //     "/auto_run_status", 10,
//     //     std::bind(&LaserObstaAvoid::PathCallback, this, std::placeholders::_1));
    
//     // odom_sub_ = this->create_subscription<agv_msgs::msg::OdometryLite>(
//     //     "/odom", 1,
//     //     std::bind(&LaserObstaAvoid::OdomCallback, this, std::placeholders::_1));
// }

bool LaserObstaAvoid::LaserReadXml(XMLElement *element)
{
    uEquipment laser_equip;
    RCLCPP_INFO_STREAM(rclcpp::get_logger("LaserObstaAvoid"), "init lidar equipment xml param");
    
    try {
        laser_equip.name = element->Attribute("name") ? stoi(string(element->Attribute("name"))) : EVENT_INT;
        laser_equip.pose = element->Attribute("pose") ? stoi(string(element->Attribute("pose"))) : EVENT_INT;
        laser_equip.topicname = element->Attribute("topicname") ? element->Attribute("topicname") : EVENT_STRING;
        laser_equip.inversion = element->Attribute("inversion") ? element->Attribute("inversion") : EVENT_STRING;

        XMLElement *area_node = element->FirstChildElement();
        while (area_node) {
            uArea area_tem;
            area_tem.name = area_node->Attribute("name") ? stoi(string(area_node->Attribute("name"))) : EVENT_INT;

            XMLElement *region_node = area_node->FirstChildElement();
            while (region_node) {
                uRegion region_tem;
                region_tem.name = region_node->Attribute("name") ? region_node->Attribute("name") : EVENT_STRING;

                XMLElement *point = region_node->FirstChildElement();
                while (point) {
                    uPoint point_tem;
                    point_tem.x = point->Attribute("x") ? atof(point->Attribute("x")) : EVENT_INT;
                    point_tem.y = point->Attribute("y") ? atof(point->Attribute("y")) : EVENT_INT;
                    region_tem.poinlist.push_back(point_tem);
                    point = point->NextSiblingElement();
                }
                area_tem.regionlist.push_back(region_tem);
                region_node = region_node->NextSiblingElement();
            }
            laser_equip.arealist[area_tem.name] = area_tem;
            area_node = area_node->NextSiblingElement();
        }
    } catch (const std::exception &e) {
        std::cerr << e.what() << '\n';
        RCLCPP_ERROR_STREAM(rclcpp::get_logger("LaserObstaAvoid"), "xml lidar read is error");
        return false;
    }

    laser_equip_list_.push_back(laser_equip);
    RCLCPP_INFO_STREAM(rclcpp::get_logger("LaserObstaAvoid"), "lidar equipment init xml param successed");

    ShowEquipParam(&laser_equip);
    return true;
}

void LaserObstaAvoid::ShowEquipParam(uEquipment *equipment)
{
    RCLCPP_INFO_STREAM(rclcpp::get_logger("LaserObstaAvoid"), "show the lidar equipment param");
    RCLCPP_INFO_STREAM(rclcpp::get_logger("LaserObstaAvoid"), "equip name:" << equipment->name << " topicname:" << equipment->topicname << " invension:" << equipment->inversion);

    for (auto item = equipment->arealist.begin(); item != equipment->arealist.end(); item++) {
        RCLCPP_INFO_STREAM(rclcpp::get_logger("LaserObstaAvoid"), "area name:" << item->first);
        for (size_t j = 0; j < item->second.regionlist.size(); j++) {
            RCLCPP_INFO_STREAM(rclcpp::get_logger("LaserObstaAvoid"), "region name: " << item->second.regionlist[j].name);
            for (size_t k = 0; k < item->second.regionlist[j].poinlist.size(); k++) {
                RCLCPP_INFO_STREAM(rclcpp::get_logger("LaserObstaAvoid"), "x:" << item->second.regionlist[j].poinlist[k].x);
                RCLCPP_INFO_STREAM(rclcpp::get_logger("LaserObstaAvoid"), "y:" << item->second.regionlist[j].poinlist[k].y);
            }
        }
    }
}

vector<LaserObstaAvoid::uPoint> LaserObstaAvoid::SplitRegion(const int &equip_name, const int &area_name, const string &region_name)
{
    vector<uPoint> poly_tem;
    for (size_t i = 0; i < laser_equip_list_.size(); i++) {
        if (laser_equip_list_[i].name == equip_name) {
            if (laser_equip_list_[i].arealist.find(area_name) != laser_equip_list_[i].arealist.end()) {
                uArea area = laser_equip_list_[i].arealist.find(area_name)->second;
                for (size_t j = 0; j < area.regionlist.size(); j++) {
                    if (area.regionlist[j].name == region_name) {
                        for (size_t k = 0; k < area.regionlist[j].poinlist.size(); k++) {
                            uPoint u_point;
                            u_point.x = area.regionlist[j].poinlist[k].x;
                            u_point.y = area.regionlist[j].poinlist[k].y;
                            poly_tem.push_back(u_point);
                        }
                        return poly_tem;
                    }
                }
            }
        }
    }

    RCLCPP_INFO_STREAM(rclcpp::get_logger("LaserObstaAvoid"), 
        "not found equip" << equip_name << " in laser_equip_list");
    return poly_tem;
}

bool LaserObstaAvoid::InPolygon(uPoint &p, std::vector<uPoint> &poly)
{
    int polySides = poly.size();
    int i, j = polySides - 1;
    double x = p.x, y = p.y;
    bool oddNodes = 0;

    for (i = 0; i < polySides; i++) {
        if ((poly[i].y < y && poly[j].y >= y || poly[j].y < y && poly[i].y >= y) &&
            (poly[i].x <= x || poly[j].x <= x)) {
            if (poly[i].x + (y - poly[i].y) / (poly[j].y - poly[i].y) * (poly[j].x - poly[i].x) < x) {
                oddNodes = !oddNodes;
            }
        }
        j = i;
    }
    return oddNodes;
}

void LaserObstaAvoid::ScanCallback(const sensor_msgs::msg::LaserScan::SharedPtr scan_msg,
                                  const string &inversion, int equip_name, rclcpp::Time time)
{
    std::lock_guard<std::mutex> sc_mutex(scan_mutex_);
    double range;
    double angle;
    uPoint scanpoint;
    vector<uPoint> point_vector;
    
    if (1 == equip_name) {
        lidar_time_1_ = time;
    } else if (2 == equip_name) {
        lidar_time_2_ = time;
    }

    int count = scan_msg->ranges.size();
    for (int k = 0; k < count; k++) {
        if (scan_msg->ranges[k] > 0.03 && scan_msg->ranges[k] < 15.0 &&
            !std::isinf(scan_msg->ranges[k]) && !std::isnan(scan_msg->ranges[k])) {
            range = scan_msg->ranges[k];
            angle = scan_msg->angle_min + scan_msg->angle_increment * k;

            if (inversion == "0") {
                // pf
                scanpoint.x = range * cos(angle);
                scanpoint.y = range * sin(angle);
            } else if (inversion == "1") {
                // sick
                scanpoint.x = range * cos(angle);
                scanpoint.y = -range * sin(angle);
            }
            point_vector.push_back(scanpoint);
        }
    }
    laser_point_.erase(equip_name);
    laser_point_[equip_name] = point_vector;
}

std::vector<agv_msgs::msg::AvoidObsta> LaserObstaAvoid::ScanCheck(vector<ScenesJudge::cEquip> &laser_equip)
{
    std::lock_guard<std::mutex> sc_mutex(scan_mutex_);
    std::vector<agv_msgs::msg::AvoidObsta> avoid_msg;
    
    for (size_t i = 0; i < laser_equip.size(); i++) {
        if (laser_point_.find(laser_equip[i].name) != laser_point_.end()) {
            agv_msgs::msg::AvoidObsta equip_avoid_msg;
            bool isWarn = false;
            bool isSlow = false;
            bool isStop = false;
            auto item = laser_point_.find(laser_equip[i].name)->second;

            std::vector<uPoint> point_warn = SplitRegion(laser_equip[i].name, laser_equip[i].area, "warn");
            std::vector<uPoint> point_slow = SplitRegion(laser_equip[i].name, laser_equip[i].area, "slow");
            std::vector<uPoint> point_stop = SplitRegion(laser_equip[i].name, laser_equip[i].area, "stop");

            bool warn_status_ = Isenable("warn", laser_equip[i]);
            bool slow_status_ = Isenable("slow", laser_equip[i]);
            bool stop_status_ = Isenable("stop", laser_equip[i]);

            for (auto item : laser_equip_list_) {
                if (laser_equip[i].name == item.name) {
                    equip_avoid_msg.pose = item.pose;
                    
                    if (((item.pose == 1) || (item.pose == 5) || (item.pose == 7)) && vehicle_forward_) {
                        warn_status_ = false;
                        slow_status_ = false;
                        stop_status_ = false;
                    }
                }
            }

            for (size_t j = 0; j < item.size(); j++) {
                isStop |= (stop_status_ == 1) && InPolygon(item[j], point_stop);
                isSlow |= (slow_status_ == 1) && InPolygon(item[j], point_slow);
                isWarn |= (warn_status_ == 1) && InPolygon(item[j], point_warn);
            }

            if (scan_check_size_) {
                isStop = LaserNumCheck(laser_equip[i].name, "stop", isStop);
                isSlow = LaserNumCheck(laser_equip[i].name, "slow", isSlow);
                isWarn = LaserNumCheck(laser_equip[i].name, "warn", isWarn);
            }

            isStop = CheckVelState(isStop, last_stop_);
            last_stop_ = isStop;

            equip_avoid_msg.equipment = laser_equip[i].name;
            equip_avoid_msg.area = laser_equip[i].area;
            equip_avoid_msg.stop = isStop ? 1 : 0;
            equip_avoid_msg.slow = isSlow ? 1 : 0;
            equip_avoid_msg.warn = isWarn ? 1 : 0;
            
            avoid_msg.push_back(equip_avoid_msg);
        }
    }
    return avoid_msg;
}

bool LaserObstaAvoid::Isenable(string status, ScenesJudge::cEquip &equip)
{
    string warn = "warn";
    string slow = "slow";
    string stop = "stop";

    if (status == warn) {
        return equip.warnenable;
    } else if (status == slow) {
        return equip.slowenable;
    } else if (status == stop) {
        return equip.stopenable;
    }

    return true;
}

bool LaserObstaAvoid::TimeoutCheck(const builtin_interfaces::msg::Time &time)
{
    if (laser_equip_list_.size() == 1) {
        if ((rclcpp::Time(time) - rclcpp::Time(lidar_time_1_)).seconds() > 2) {
            return true;
        }
    } else if (laser_equip_list_.size() == 2) {
        if ((rclcpp::Time(time) - rclcpp::Time(lidar_time_1_)).seconds() > 2 ||
            (rclcpp::Time(time) - rclcpp::Time(lidar_time_2_)).seconds() > 2) {
            return true;
        }
    }

    return false;
}

bool LaserObstaAvoid::LaserNumCheck(int &equip_name, const string &region_name, bool &flag)
{
    if (equip_name == 1) {
        if (region_name == "stop") {
            if (flag) {
                if (lidar_1_.stop_num < num_frame_) {
                    lidar_1_.stop_num++;
                    return false;
                } else {
                    return true;
                }
            } else {
                lidar_1_.stop_num = 0;
                return false;
            }
        }

        if (region_name == "slow") {
            if (flag) {
                if (lidar_1_.slow_num < num_frame_) {
                    lidar_1_.slow_num++;
                    return false;
                } else {
                    return true;
                }
            } else {
                lidar_1_.slow_num = 0;
                return false;
            }
        }

        if (region_name == "warn") {
            if (flag) {
                if (lidar_1_.warn_num < num_frame_) {
                    lidar_1_.warn_num++;
                    return false;
                } else {
                    return true;
                }
            } else {
                lidar_1_.warn_num = 0;
                return false;
            }
        }
    }

    if (equip_name == 2) {
        if (region_name == "stop") {
            if (flag) {
                if (lidar_2_.stop_num < num_frame_) {
                    lidar_2_.stop_num++;
                    return false;
                } else {
                    return true;
                }
            } else {
                lidar_2_.stop_num = 0;
                return false;
            }
        }

        if (region_name == "slow") {
            if (flag) {
                if (lidar_2_.slow_num < num_frame_) {
                    lidar_2_.slow_num++;
                    return false;
                } else {
                    return true;
                }
            } else {
                lidar_2_.slow_num = 0;
                return false;
            }
        }

        if (region_name == "warn") {
            if (flag) {
                if (lidar_2_.warn_num < num_frame_) {
                    lidar_2_.warn_num++;
                    return false;
                } else {
                    return true;
                }
            } else {
                lidar_2_.warn_num = 0;
                return false;
            }
        }
    }

    return true;
}

void LaserObstaAvoid::PathCallback(const agv_msgs::msg::AgvAutoStatus::SharedPtr path_msg)
{
    bool wheel_forward = false;
    bool traj_line = false;

    if (path_msg->wheelmovedir == 0) {
        wheel_forward = true;
    }
    
    if (path_msg->trajtype == 0) {
        traj_line = true;
    }
    
    if (wheel_forward && traj_line) {
        vehicle_forward_ = true;
    } else {
        vehicle_forward_ = false;
    }
}

void LaserObstaAvoid::OdomCallback(const agv_msgs::msg::OdometryLite::SharedPtr odom_msg)
{
    cur_speed_[0] = odom_msg->twist.linear.x;
    cur_speed_[1] = odom_msg->twist.linear.y;
    cur_speed_[2] = odom_msg->twist.angular.z;
}

bool LaserObstaAvoid::CheckVelState(const bool &cur_state, const bool &last_state)
{
    bool stop_state = false;

    if (cur_state) {
        return true;
    }

    if (last_state) {
        for (size_t i = 0; i < 3; i++) {
            if (abs(cur_speed_[i]) > 0.006) {
                stop_state = true;
            }
        }
        RCLCPP_INFO_STREAM(rclcpp::get_logger("LaserObstaAvoid"), 
            "odom vx:" << cur_speed_[0] << " vy:" << cur_speed_[1] << " w:" << cur_speed_[2]);
        RCLCPP_INFO_STREAM(rclcpp::get_logger("LaserObstaAvoid"), 
            "logic stop state:" << stop_state);
    }

    return stop_state;
}

} // namespace AgvObstaAvoid